'''主控程序'''
import threading
from module_order import HttpOrder
from module_robot import HandRobot
from module_vision import StereoVision
from module_show import ShowCamare
from configs import *
import time,cv2,copy
from module_mqtt import MqttPub

#TODO 重复倒菜的问题修复;细小误差不偏移


class MainController:
    def __init__(self):
        self.args = parse_args()
        self.handrobot = HandRobot(self.args.robot_ip)
        self.flow=[False,False,False]
        self.stereovision = StereoVision()
        self.last_mqtt_plate = {}
        self.last_mqtt_pot = {}
        self.cook_count = 0
        self.order_finish = 0#0,1,2 未开始，进行中，结束
        self.last_world_coord = []
        self.current_vision_result={}#取完菜之后记录当前状态，直到菜打完重置
        self.prepare_daocai=False #准备倒菜
    def visiondata2mqttdata(self,vision_result):
        #视觉结果转换成mqtt需要的格式
        mqtt_plate = copy.deepcopy(self.args.mqtt_plate)
        mqtt_pot = copy.deepcopy(self.args.mqtt_pot)
        #TODO 判断是否为空
        for label in vision_result["labels"]:
            if label == "0":
                mqtt_plate["plate_exist"]=1
            else:
                try:
                    cla,pos,empty = label.split("_")
                except:
                    pass
                else:
                    if cla == "p":
                        if pos in plate_pos and pos !='21':
                            mqtt_plate["plate_empty"][pos]=empty#TODO 目前一直是0,表示空
                    elif cla == "pot":
                        # print(pos)
                        if pos in pot_pos and empty in list(self.args.menuid_name.keys()):
                            mqtt_pot["pot_menu"][pot_pos_screen[pos]][empty] = self.args.menuid_name[empty]
        return mqtt_plate,mqtt_pot

    # def visiondata2orderdata(self,order):
        # order_values = list(order.values())[0]
        # orderdata = [] #orders = {"bianhao":[["bianhao","caiming"],["",""]]}
        # i = 0
        # count=0
        # print("stereovision_pot_msg",self.stereovision.pot_msg)#{'0': '31', 'MDJ265': '11', 'MDJ867': '12'}
        # plate_msg_sorted = [[k,self.stereovision.plate_msg[k]] for k in sorted(self.stereovision.plate_msg.keys())]
        # print("plate_msg_sorted",plate_msg_sorted)#[['11', '1'], ['12', '0'], ['13', '0'], ['21', '0'], ['22', '0']]
        # for label in plate_msg_sorted:
        #     if self.stereovision.plate_exisit_flag:
        #         pos,empty = label
        #         if empty=="0" :
        #             for key,value in self.stereovision.pot_msg.items():
        #                 if value==order_values[count][0]:
        #                     orderdata.append([pos,key])
        #                     count = count+1
        #                     break
        #         if count>=len(order_values):break
        # return orderdata

    def visiondata2orderpot(self,order):#返回打菜点list

        #order = {"bianhao":[["bianhao","caiming"],["",""]]}
        order_values = list(order.values())[0]#[["bianhao","caiming"],["",""]]
        orderpot = [] 
        self.stereovision.plat_pot_clock.acquire()
        # print("stereovision_pot_msg",self.stereovision.pot_msg)#{ '11': 'MDJ265', '12': 'MDJ867'}
        # self.stereovision.pot_msg={"11":"MDJ777","12":"MDJ867","21":"MDJ265","22":"MDJ001"}
        for ov in order_values:
            for key,value in self.stereovision.pot_msg.items():
                if value==ov[0]:
                    orderpot.append(key)
                    break
        self.stereovision.plat_pot_clock.release()
        return orderpot

    def visiondata2orderplate(self):#返回出菜点list
        orderplate = [] 
        self.stereovision.plat_pot_clock.acquire()
        plate_msg_sorted = [[k,self.stereovision.plate_msg[k]] for k in sorted(self.stereovision.plate_msg.keys())]#保证打菜顺序
        self.stereovision.plat_pot_clock.release()
        # print("plate_msg_sorted",plate_msg_sorted)#[['11', '1'], ['12', '0'], ['13', '0'], ['21', '0'], ['22', '0']]
        if self.stereovision.plate_exisit_flag:
            for pos,empty in plate_msg_sorted:
                if not int(empty) and pos !='21':#空
                    orderplate.append(pos)
        # print("orderplate",orderplate)
        return orderplate

    def label2pos(self,label):
        #通过label提取pos
        try:
            cla,pos,empty = label.split("_")
        except:
            return ""
        else:
            return (cla,pos)

    def check_same_point(self,current_world_coord):
        if bool(self.last_world_coord) and bool(current_world_coord):
            if abs(self.last_world_coord[0]-current_world_coord[0])<5 and abs(self.last_world_coord[1]-current_world_coord[1])<5:#5毫米范围内不修正
                return True
            else:
                return False
        else:
            return False

    def send_to_mqtt(self):
        while True:
            time.sleep(0.5)
            mqtt_plate,mqtt_pot = self.visiondata2mqttdata(self.stereovision.current_result)
            # print("mqtt_plate",mqtt_plate)
            # print("mqtt_pot",mqtt_pot)
            # print("last_mqtt_plate",self.last_mqtt_plate)
            # print("last_mqtt_pot",self.last_mqtt_pot)
            if self.last_mqtt_plate!=mqtt_plate:
                self.last_mqtt_plate = mqtt_plate
                mq.pub(self.args.mqtt_topic_palte_pub,mqtt_plate)

            # mqtt_pot ={"pot_menu":{"11":{"CLK124":"方腿毛豆"},"12":{"MDJ777":"素肠毛豆肉片"},"21":{"MDJ265":"韭菜鸡蛋"},"22":{"MDJ651":"茭白肉丝"},"31":{"MDJ001":"山药木耳肉片"},"32":{"MDJ867":"青豆玉米虾仁"}}}

            if self.last_mqtt_pot!=mqtt_pot:# 餐盒状态有变化时发送
                self.last_mqtt_pot = mqtt_pot
                mq.pub(self.args.mqtt_topic_pot_pub,mqtt_pot)

    # def get_push_out(self,in_plate,out_plate):#in_plate取餐-out_plate出餐-到餐

    #     if self.stereovision.plate_exisit_flag:#前提是有餐盘放入
    #         if not self.flow[0]:#未取餐

    #             if self.handrobot.robot_do(self.args.pot_robot_map[in_plate],msg='取餐'):#取餐完成
    #                 self.flow[0],self.flow[1]= True,True
    #             else:
    #                 pass# 取餐完成之前会一直发送取餐指令
    #         if self.flow[1]:#已取餐
    #             for bbox, label_id in zip(self.stereovision.current_result["bbox"], self.stereovision.current_result["labels"]):
    #                 c_pos = self.label2pos(label_id)
    #                 if bool(c_pos):#餐格
    #                     if  c_pos[0]=='p' and c_pos[1]==out_plate:
    #                         out_plate_center = (int((bbox[0]+bbox[2])/2),int((bbox[1]+bbox[3])/2))
    #                         world_coord = self.stereovision.pixel2world(out_plate_center)
    #                         print(label_id,world_coord)
    #                         if self.handrobot.robot_do_coord(world_coord,"送餐到指定位置"):
    #                             self.stereovision.last_result = copy.deepcopy(self.stereovision.current_result)
    #                             if self.stereovision.daocai_ready:
    #                                 self.flow[2]= True
    #                                 break
    #         if self.flow[2]:#已出餐
    #             if self.handrobot.daocai(self.stereovision.daocai_ready):#
    #                 self.flow = [False,False,False]#倒菜完成
    #                 self.cook_finish = True
    #                 # break
    def process(self):
        cook_id=0
        cook_id_get=False
        while True:
            time.sleep(0.1)
            time_current = time.time()
            if self.stereovision.ready and self.handrobot.ready and bool(http_order.current_order):#视觉，机器人，订单就绪
                uuid = list(http_order.current_order.keys())[0]

                # print('orderpot,orderplate',orderpot,orderplate)
                if self.stereovision.plate_exisit_flag and self.order_finish!=2:#订单未结束
                    if not self.flow[0]:#未取餐
                        orderpot = self.visiondata2orderpot(http_order.current_order)
                        # orderplate = self.visiondata2orderplate()#TODO

                        if len(orderpot):#取餐位置有目标菜
                            # print(44444444444)
                            self.order_finish=1#订单进行中
                            http_order.updata_statu(uuid,2)
                            try:
                                if not cook_id_get:
                                    cook_id = self.args.pot_robot_map[orderpot[self.cook_count]]
                            except:
                                pass
                            else:
                                cook_id_get=True
                                self.handrobot.set_robot_do(cook_id,msg='设置从{}取餐命令'.format(str(cook_id)))#设置取餐命令
                        if self.handrobot.get_robot_do_done(cook_id,msg="从{}开始取餐".format(str(cook_id))):#取餐完成
                            logger.info("从{}取餐完成".format(str(cook_id)))
                            self.flow[0],self.flow[1]= True,True
                            cook_id_get=False

                    
                    if self.flow[1]:#已取餐
                        orderplate = self.visiondata2orderplate()#TODO
                        for bbox, label_id in zip(self.stereovision.current_result["bbox"], self.stereovision.current_result["labels"]):
                            c_pos = self.label2pos(label_id)
                            print(222)
                            if bool(c_pos) and len(orderplate):#餐格
                                if c_pos[0]=='p' and c_pos[1]==orderplate[0]:#TODO 找到目标出餐格，可能在运行到目标位置的过程中误判餐格已占用
                                    out_plate_center = (int((bbox[0]+bbox[2])/2),int((bbox[1]+bbox[3])/2))
                                    world_coord = self.stereovision.pixel2world(out_plate_center)
                                    print(world_coord)
                                    if not self.check_same_point(world_coord):
                                        # print(label_id,world_coord)
                                        # logger.info("出餐到{}-{}".format(str(c_pos[1]),world_coord))
                                        self.last_world_coord=copy.deepcopy(world_coord)
                                        
                                        self.handrobot.set_robot_do_coord(world_coord,"设置出餐位置{}-{}".format(str(c_pos[1]),world_coord))
                                    # else:
                                    #     self.last_world_coord=copy.deepcopy(world_coord)
                                    print('2',self.last_world_coord,world_coord)
                                    if self.handrobot.set_robot_do_coord_flag:
                                        self.handrobot.set_robot_do(7,msg="设置出餐动作到{}-{}".format(str(c_pos[1]),world_coord))#设置出餐命令
                        if self.handrobot.get_robot_do_done(7,msg="出餐动作"):#出餐完成,可能反复设置出餐位置
                            self.handrobot.set_robot_do_coord_flag=False
                            self.stereovision.last_result = copy.deepcopy(self.stereovision.current_result)
                            self.stereovision.check_daocai_ready()
                            self.prepare_daocai=True
                        if self.prepare_daocai and self.stereovision.daocai_ready:
                            logger.info("{}允许到餐".format(world_coord))
                            self.flow[2]= True#跳出当前步
                    if self.flow[2]:#已出餐
                        self.flow[1]=False
                        
                        if self.handrobot.daocai(self.stereovision.daocai_ready):#
                            self.flow = [False,False,False]#倒菜完成
                            self.prepare_daocai=False
                            self.last_world_coord = []
                            self.current_vision_result={}
                            self.stereovision.last_result = {"labels":[],"scores":[],"bbox":[]}
                            self.cook_count = self.cook_count+1
                # print(self.cook_count)
                #订单结束处理
                if self.cook_count==http_order.current_order_len :#订单存在且完成
                    self.cook_count =0
                    self.order_finish=2
                    
                    http_order.updata_statu(uuid,3)
                    logger.info("当前订单{}完成".format(str(uuid)))
                if  not self.stereovision.plate_exisit_flag and self.order_finish==2:
                    logger.info("当前订单{}已取餐".format(str(uuid)))
                    http_order.updata_statu(uuid,4)
                    http_order.current_order={}
                    self.order_finish=0
                    http_order.oldordrlist.clear()#重新接单
                    
                    
                    # self.handrobot.set_robot_do(9,msg = "设置回安全位置")
                #     if self.handrobot.get_robot_do_done(9,"执行回安全位置"):
                #         # pass
                        
                #         self.order_finish=True
                #  and self.order_finish:


if __name__ == "__main__":
    controller = MainController()
    http_order = HttpOrder()
    mq = MqttPub()
    show_vision = ShowCamare(controller.stereovision)

    threading.Thread(target=show_vision.show).start()

    threading.Thread(target=http_order.get_order).start()

    threading.Thread(target=controller.stereovision.return_vision_result).start()

    threading.Thread(target=controller.process).start()

    threading.Thread(target=controller.send_to_mqtt).start()

    # threading.Thread(target=mq.client.loop_forever).start()
    mq.client.loop_forever()
    


